/*
    minimize
    n   liczba zmiennych
    x   zmienne wynikowe
    x0  punkt startowy
    max ograniczenie iteracji
*/
int bfgs(OptimFunc of, int n, double*x, double*x0, int max ){
    int i, iter;
    double xprev[n];
    double g[n];
    double minusG[n];
    double g1[n];
    double alfa = EPS;  // dlugosc kroku
    double d[n];           // kierunek poszukiwañ
    double *h[n];     // hesian
    double s[n];        // sk = xk+1 - xk
    double y[n];        // yk = grad(fk+1) - grad(fk)
    double pk;          // pk= 1/(yk'*sk)
    double *v[n];
    double *pkyksk[n];
    double *hl[n];

    for (i=0;i<n;++i){
        h[i] = malloc(sizeof(double) * n);
        v[i] = malloc(sizeof(double) * n);
        pkyksk[i] = malloc(sizeof(double) * n);
        hl[i] = malloc(sizeof(double) * n);
    }
    initMatrixIdentity(h, n, 1.0);
    mov(x,x0,n); // x = x0
    for (iter=0; iter<max; ++iter){
        //of(x, n);
        gradient(of, n, x, g);  // g = gradient
        mulMatrixByVector(h, g, n, d);
        movNeg(d, d, n);  // d = -d
        movNeg(minusG, g, n);  // minusG = -g
        printV("d", d, n);
        printV("g", g, n);
        alfa = findAlfaBisection(of, n, x, d, MAX_JUMP, 100);
        //printf("alfa %g\n", alfa);
        if (alfa == 0.0){
            printf("alfa = 0, stopping\n");
            printf("hesian\n");
            printMatrix(h, n);
            printf("pochodna w kierunku d: %g\n", directedG(of,n,x,d,0));
            printf("pochodna w kierunku -gradient: %g\n", directedG(of,n,x,minusG,0));
            printf("krokow: %d\n", iter);
            return 0;
        }
        printf("hesian %d\n", iter);
        printMatrix(h, n);
        mov(xprev, x, n);   // xprev = x
        for (i=0; i<n; ++i) x[i] = x[i] + alfa*d[i];    // x = x + alfa*d
        printf("%d ", iter);
        printV("x", x, n);
        /// update hesian variables
        subVectors(x, xprev, n, s);
        gradient(of, n, x, g1);  // g = gradient(xk+1)
        subVectors(g1, g, n, y);
        pk = 1 / (float)mulVectorTransByVector(y, s, n);
        initMatrixIdentity(v, n, 1.0);
        mulVectorByVectorTrans(y, s, n, pkyksk);
        mulMatrixByConstant(pkyksk, n, pk, pkyksk);
        subMatrix(v, pkyksk, n, v);
        /// update hesian
        mulMatrixByMatrix(h, v, n, hl);
        transposeMatrix(v, n, v);
        mulMatrixByMatrix(v, hl, n, h); // lewa strona gotowa i zapisana w h, wolne hl
        mulVectorByVectorTrans(s, s, n, hl);
        mulMatrixByConstant(hl, n, pk, hl); // prawa strona gotowa i zapisana w hl
        addMatrix(h, hl, n, h);
        //initMatrixIdentity(h, n, 1.0);
    }
    for (i=0;i<n;++i){
        free(hl[i]);
        free(pkyksk[i]);
        free(v[i]);
        free(h[i]);
    }
    return 0;
}
